#include "dance.h"


// 身体前后摇摆
void Dance_ShakeBody(uint8_t times)
{
    for (uint8_t i = 0; i < times; i++) {
        // 向前倾
        Servo_Set_Target_Angle(0, 60);   // 左前腿向前
        Servo_Set_Target_Angle(1, 120);  // 右后腿向后
        Servo_Set_Target_Angle(2, 60);   // 右前腿向前
        Servo_Set_Target_Angle(3, 120);  // 左后腿向后
        delay_ms(200);
        
        // 向后仰
        Servo_Set_Target_Angle(0, 120);  // 左前腿向后
        Servo_Set_Target_Angle(1, 60);   // 右后腿向前
        Servo_Set_Target_Angle(2, 120);  // 右前腿向后
        Servo_Set_Target_Angle(3, 60);   // 左后腿向前
        delay_ms(200);
        
        // 回到中立
        Servo_Set_Target_Angle(0, 90);
        Servo_Set_Target_Angle(1, 90);
        Servo_Set_Target_Angle(2, 90);
        Servo_Set_Target_Angle(3, 90);
        delay_ms(100);
    }
}

// 转圈动作
void Dance_Spin(uint8_t circles)
{
    for (uint8_t circle = 0; circle < circles; circle++) {
        // 顺时针转圈（分4步完成一个圆圈）
        for (uint8_t step = 0; step < 4; step++) {
            // 抬起对角腿
            Servo_Set_Target_Angle(0, 40);   // 左前腿抬起
            Servo_Set_Target_Angle(1, 140);  // 右后腿抬起
            delay_ms(150);
            
            // 向前移动
            Servo_Set_Target_Angle(0, 60);
            Servo_Set_Target_Angle(1, 120);
            delay_ms(100);
            
            // 放下
            Servo_Set_Target_Angle(0, 90);
            Servo_Set_Target_Angle(1, 90);
            delay_ms(100);
            
            // 抬起另一对角腿
            Servo_Set_Target_Angle(2, 140);  // 右前腿抬起
            Servo_Set_Target_Angle(3, 40);   // 左后腿抬起
            delay_ms(150);
            
            // 向前移动
            Servo_Set_Target_Angle(2, 120);
            Servo_Set_Target_Angle(3, 60);
            delay_ms(100);
            
            // 放下
            Servo_Set_Target_Angle(2, 90);
            Servo_Set_Target_Angle(3, 90);
            delay_ms(100);
        }
    }
}

// 波浪动作（腿依次抬起）
void Dance_Wave(uint8_t times)
{
    for (uint8_t i = 0; i < times; i++) {
        // 波浪从右前腿开始，逆时针传播
        Servo_Set_Target_Angle(2, 120);  // 右前腿抬起
        delay_ms(100);
        Servo_Set_Target_Angle(2, 90);   // 放下
        delay_ms(50);
        
        Servo_Set_Target_Angle(0, 120);  // 左前腿抬起
        delay_ms(100);
        Servo_Set_Target_Angle(0, 90);   // 放下
        delay_ms(50);
        
        Servo_Set_Target_Angle(3, 60);   // 左后腿抬起
        delay_ms(100);
        Servo_Set_Target_Angle(3, 90);   // 放下
        delay_ms(50);
        
        Servo_Set_Target_Angle(1, 60);   // 右后腿抬起
        delay_ms(100);
        Servo_Set_Target_Angle(1, 90);   // 放下
        delay_ms(50);
        
        // 反向波浪（顺时针）
        Servo_Set_Target_Angle(1, 60);   // 右后腿抬起
        delay_ms(80);
        Servo_Set_Target_Angle(1, 90);
        delay_ms(40);
        
        Servo_Set_Target_Angle(3, 60);   // 左后腿抬起
        delay_ms(80);
        Servo_Set_Target_Angle(3, 90);
        delay_ms(40);
        
        Servo_Set_Target_Angle(0, 120);  // 左前腿抬起
        delay_ms(80);
        Servo_Set_Target_Angle(0, 90);
        delay_ms(40);
        
        Servo_Set_Target_Angle(2, 120);  // 右前腿抬起
        delay_ms(80);
        Servo_Set_Target_Angle(2, 90);
        delay_ms(100);
    }
}

// 上下跳动
void Dance_Jump(uint8_t times)
{
    for (uint8_t i = 0; i < times; i++) {
        // 下蹲准备
        Servo_Set_Target_Angle(0, 70);   // 所有腿弯曲
        Servo_Set_Target_Angle(1, 110);
        Servo_Set_Target_Angle(2, 70);
        Servo_Set_Target_Angle(3, 110);
        delay_ms(200);
        
        // 跳起（腿伸直）
        Servo_Set_Target_Angle(0, 100);
        Servo_Set_Target_Angle(1, 80);
        Servo_Set_Target_Angle(2, 100);
        Servo_Set_Target_Angle(3, 80);
        delay_ms(150);
        
        // 落地缓冲
        Servo_Set_Target_Angle(0, 85);
        Servo_Set_Target_Angle(1, 95);
        Servo_Set_Target_Angle(2, 85);
        Servo_Set_Target_Angle(3, 95);
        delay_ms(100);
        
        // 回到中立
        Servo_Set_Target_Angle(0, 90);
        Servo_Set_Target_Angle(1, 90);
        Servo_Set_Target_Angle(2, 90);
        Servo_Set_Target_Angle(3, 90);
        delay_ms(100);
    }
}

// 结束姿势（酷炫的定型动作）
void Dance_FinalPose(void)
{
    // 抬起右前腿和左后腿，摆出帅气姿势
    Servo_Set_Target_Angle(0, 90);   // 左前腿支撑
    Servo_Set_Target_Angle(1, 90);   // 右后腿支撑
    Servo_Set_Target_Angle(2, 150);  // 右前腿高高抬起
    Servo_Set_Target_Angle(3, 30);   // 左后腿高高抬起
    delay_ms(1000);
    
    // 缓慢放下
    for (uint8_t angle = 150; angle > 90; angle -= 5) {
        Servo_Set_Target_Angle(2, angle);
        Servo_Set_Target_Angle(3, 180 - angle);
        delay_ms(50);
    }
    
    // 回到完全中立位置
    Servo_Set_Target_Angle(0, 90);
    Servo_Set_Target_Angle(1, 90);
    Servo_Set_Target_Angle(2, 90);
    Servo_Set_Target_Angle(3, 90);
    delay_ms(500);
}

// 简单的节奏动作（可以配合音乐）
void Dance_RhythmBeat(uint8_t beats)
{
    for (uint8_t i = 0; i < beats; i++) {
        // 快速抬起前腿打节奏
        Servo_Set_Target_Angle(0, 120);
        Servo_Set_Target_Angle(2, 120);
        delay_ms(80);
        
        Servo_Set_Target_Angle(0, 90);
        Servo_Set_Target_Angle(2, 90);
        delay_ms(80);
        
        // 快速抬起后腿打节奏
        Servo_Set_Target_Angle(1, 60);
        Servo_Set_Target_Angle(3, 60);
        delay_ms(80);
        
        Servo_Set_Target_Angle(1, 90);
        Servo_Set_Target_Angle(3, 90);
        delay_ms(80);
    }
}

// 跳舞动作序列
void SG90_Dance_Advance(void)
{
    // 舞蹈1：前后摇摆（准备动作）
    Dance_ShakeBody(3);
    
    // 舞蹈2：转圈
    Dance_Spin(2);
    
    // 舞蹈3：波浪动作
    Dance_Wave(2);
    
    // 舞蹈4：上下跳动
    // Dance_Jump(3);
    
    // 舞蹈5：结束姿势
    Dance_FinalPose();
}
